using System;
using System.Collections.Generic;
using Priority_Queue;
using UnityEngine;

namespace Gj.Pathfinding
{
    public interface JPSInterface
    {
        Vector2Int GetDirection(Vector2Int position, byte dir);
        float Distance(Vector2Int current, Vector2Int _goal);
        byte[] ExtendDir(byte sourceDir, byte extendDir);

        int DirectionCount();
        bool ForcedNeighbours(JPS jps, JPSNode node, int mask, ref byte dirs);
        byte NaturalNeighbours(JPS jps, byte dir);
    }

    internal class SquareJPSInstance : JPSInterface
    {
        internal const float SQRT2 = 1.414213562f;

        private readonly Vector2Int[] _d =
        {
            new(0, 1), // N 0
            new(1, 1), // NE 1
            new(1, 0), // E 2
            new(1, -1), // SE 3
            new(0, -1), // S 4
            new(-1, -1), // SW 5
            new(-1, 0), // W 6
            new(-1, 1) // NW 7
        };

        public Vector2Int GetDirection(Vector2Int position, byte dir)
        {
            return _d[dir];
        }

        public float Distance(Vector2Int current, Vector2Int _goal)
        {
            return (current - _goal).magnitude;
        }

        public byte[] ExtendDir(byte sourceDir, byte currentDir)
        {
            if (IsDiagonal(currentDir)) return new[] { (byte)((currentDir + 1) % 8), (byte)((currentDir + 8 - 1) % 8) };

            return new byte[] { };
        }

        public int DirectionCount()
        {
            return 8;
        }

        public bool ForcedNeighbours(JPS jps, JPSNode node, int mask, ref byte dirs)
        {
            if (node.Dir == 255)
                return false;
            var flag = false;
            if (IsDiagonal(node.Dir))
            {
                if (jps.ENTERABLE(node, node.Dir, 6, mask) && !jps.ENTERABLE(node, node.Dir, 5, mask))
                {
                    dirs = jps.AddDirectionToSet(dirs, (byte)((node.Dir + 6) % 8));
                    flag = true;
                }

                if (jps.ENTERABLE(node, node.Dir, 2, mask) && !jps.ENTERABLE(node, node.Dir, 3, mask))
                {
                    dirs = jps.AddDirectionToSet(dirs, (byte)((node.Dir + 2) % 8));
                    flag = true;
                }
            }
            else
            {
                if (jps.ENTERABLE(node, node.Dir, 7, mask) && !jps.ENTERABLE(node, node.Dir, 6, mask))
                {
                    dirs = jps.AddDirectionToSet(dirs, (byte)((node.Dir + 7) % 8));
                    flag = true;
                }

                if (jps.ENTERABLE(node, node.Dir, 1, mask) && !jps.ENTERABLE(node, node.Dir, 2, mask))
                {
                    dirs = jps.AddDirectionToSet(dirs, (byte)((node.Dir + 1) % 8));
                    flag = true;
                }
            }

            return flag;
        }

        public byte NaturalNeighbours(JPS jps, byte dir)
        {
            if (dir == 255)
                return 255;

            byte dirs = 0;
            dirs = jps.AddDirectionToSet(dirs, dir);
            if (IsDiagonal(dir))
            {
                dirs = jps.AddDirectionToSet(dirs, (byte)((dir + 1) % 8));
                dirs = jps.AddDirectionToSet(dirs, (byte)((dir + 8 - 1) % 8));
            }
            else
            {
                dirs = jps.AddDirectionToSet(dirs, (byte)((dir + 2) % 8));
                dirs = jps.AddDirectionToSet(dirs, (byte)((dir + 8 - 2) % 8));
            }

            return dirs;
        }

        public float DiagonalCost()
        {
            return SQRT2;
        }

        public bool IsDiagonal(byte dir)
        {
            return dir % 2 != 0;
        }


        /// 估价函数
        private float Heuristic(Vector2Int current, Vector2Int _goal)
        {
            var diff = _goal - current;
            diff.x = Mathf.Abs(diff.x);
            diff.y = Mathf.Abs(diff.y);
            return Mathf.Max(diff.x, diff.y) + (SQRT2 - 1) * Mathf.Min(diff.x, diff.y);
        }
    }

    internal class HexJPSInstance : JPSInterface
    {
        private Vector2Int[] _d;
        private readonly HexMetrics _metrics;

        public HexJPSInstance(HexMetrics metrics)
        {
            _metrics = metrics;
        }

        public Vector2Int GetDirection(Vector2Int position, byte dir)
        {
            var p = new Vector2Int();
            if (_metrics.Mode == HexMode.Column)
                HexCube.WithDirection(position, (HexDirectionColumn)dir, ref p);
            else
                HexCube.WithDirection(position, (HexDirectionRow)dir, ref p);
            return p;
        }

        public float Distance(Vector2Int current, Vector2Int _goal)
        {
            // Debug.LogError($"distance: {current} {_goal} {(Math.Abs(current.x - _goal.x) + Math.Abs(current.y - _goal.y) )}");
            return (Math.Abs(current.x - _goal.x) + Math.Abs(current.y - _goal.y) );
        }

        // public bool IsDiagonal(byte dir)
        // {
        //     if (_mode == HexMode.Column)
        //     {
        //         return dir != 1 && dir != 4;
        //     }
        //     else
        //     {
        //         return dir != 2 && dir != 5;
        //     }
        // }
        public byte[] ExtendDir(byte sourceDir, byte currentDir)
        {
            if (currentDir == sourceDir)
                return new byte[] { };
            return new[] { (byte)((currentDir - sourceDir + 6) % 6) };
        }

        public int DirectionCount()
        {
            return 6;
        }

        public bool ForcedNeighbours(JPS jps, JPSNode node, int mask, ref byte dirs)
        {
            if (node.Dir == 255)
                return false;
            var flag = false;
            if (jps.ENTERABLE(node, node.Dir, 5, mask) && !jps.ENTERABLE(node, node.Dir, 4, mask))
            {
                dirs = jps.AddDirectionToSet(dirs, (byte)((node.Dir + 5) % 6));
                flag = true;
            }

            if (jps.ENTERABLE(node, node.Dir, 1, mask) && !jps.ENTERABLE(node, node.Dir, 2, mask))
            {
                dirs = jps.AddDirectionToSet(dirs, (byte)((node.Dir + 1) % 6));
                flag = true;
            }

            // if (jps.ENTERABLE(node, dir, 2, mask) && !jps.ENTERABLE(node, dir, 3, mask))
            // {
            //     dirs = jps.AddDirectionToSet(dirs, (byte) ((dir + 2) % 6));
            //     flag = true;
            // }
            return flag;
        }

        public byte NaturalNeighbours(JPS jps, byte dir)
        {
            if (dir == 255)
                return 255;

            byte dirs = 0;
            dirs = jps.AddDirectionToSet(dirs, dir);
            var d = (byte)((dir + 1) % 6);
            dirs = jps.AddDirectionToSet(dirs, d);
            // if (IsDiagonal(d))
            // {
            //     dirs = jps.AddDirectionToSet (dirs, (byte)((dir + 2) % 6));
            // }

            d = (byte)((dir + 6 - 1) % 6);
            dirs = jps.AddDirectionToSet(dirs, d);
            // if (IsDiagonal(d))
            // {
            //     dirs = jps.AddDirectionToSet (dirs, (byte)((dir + 6-2) % 6));
            // }
            return dirs;
        }

        public float DiagonalCost()
        {
            return 1;
        }
    }


    public class JPSNode : FastPriorityQueueNode
    {
        public byte Dir;
        public float Distance;

        public Vector2Int Location;
        public JPSNode Parent;
        public int Step;
        public List<Vector2Int> Source;

        public JPSNode(int x, int y) : this(new Vector2Int(x, y))
        {
        }

        public JPSNode(Vector2Int l)
        {
            Location = l;
            Source = new List<Vector2Int>();
        }

        public float Cost => Distance + Step;

        public void Revert()
        {
            Distance = 0;
            Step = 0;
            Dir = 255;
            Parent = null;
            Source.Clear();
        }

        public override bool Equals(object obj)
        {
            return Location == ((JPSNode)obj).Location;
        }

        public void Offset(Vector2Int offset)
        {
            Source.Add(Location);
            Location.x += offset.x;
            Location.y += offset.y;
        }

        public void Update(JPSNode node)
        {
            Step = node.Step;
            Distance = node.Distance;
        }
    }

    public class JPS
    {
        private static readonly Queue<JPSNode> _nodePool = new();
        private readonly List<Vector2Int> _close;

        internal readonly JPSInterface _instance;
        private readonly FastPriorityQueue<JPSNode> _open;
        private readonly Dictionary<Vector2Int, JPSNode> _openList;

        internal JPSGrid _grid;

        public JPS(JPSInterface instance, int cap = 256)
        {
            _instance = instance;
            _open = new FastPriorityQueue<JPSNode>(cap);
            _openList = new Dictionary<Vector2Int, JPSNode>();
            _close = new List<Vector2Int>(cap);
        }

        internal JPSNode NewNode(int x, int y, byte dir)
        {
            JPSNode node;
            if (_nodePool.Count > 0)
            {
                node = _nodePool.Dequeue();
                node.Location = new Vector2Int(x, y);
                node.Step = 0;
                node.Source.Clear();
                node.Parent = null;
            }
            else
            {
                node = new JPSNode(x, y) { Step = 0 };
            }

            node.Dir = dir;

            return node;
        }

        internal JPSNode NextNode(JPSNode c, byte dir)
        {
            var node = NewNextNode(c.Location, dir);
            node.Parent = c;
            node.Distance = _instance.Distance(node.Location, c.Location);
            node.Step = c.Step + 1;
            return node;
        }

        internal JPSNode NewNextNode(Vector2Int position, byte dir)
        {
            var a = _instance.GetDirection(position, dir);
            var node = NewNode(position.x + a.x, position.y + a.y, dir);

            return node;
        }

        internal byte AddDirectionToSet(byte dirs, byte dir)
        {
            return (byte)(dirs | (1 << dir));
        }

        internal bool ENTERABLE(JPSNode node, byte dir, byte n, int mask)
        {
            var a = _instance.GetDirection(node.Location, (byte)((dir + n) % _instance.DirectionCount()));
            var r = _grid._N(node.Location.x + a.x, node.Location.y + a.y, mask);
            // Console.WriteLine(node.Location+",dir:"+dir+",n:"+n+","+next.Location+","+r);
            return r;
        }

        public bool FindPath(JPSGrid grid, Vector2Int start, Vector2Int goal, int mask, ref List<Vector2Int> result)
        {
            _grid = grid;
            result.Clear();

            if (!_grid._N(start.x, start.y, mask) || !_grid._N(goal.x, goal.y, mask))
                return false;
            if (start == goal)
                return true;

            //Ready the openlist, closed list and node container for search
            _open.Clear();
            _openList.Clear();
            _close.Clear();

            //Insert the start nodes into the openList
            var startNode = NewNode(start.x, start.y, 255);
            _open.Enqueue(startNode, startNode.Cost);
            _openList.Add(startNode.Location, startNode);

            //Keep iterating over openlist until a solution is found or list is empty
            while (_open.Count > 0)
            {
                var currentNode = _open.Dequeue();
                _openList.Remove(currentNode.Location);
                _close.Add(currentNode.Location);

                var dirs = _instance.NaturalNeighbours(this, currentNode.Dir);
                _instance.ForcedNeighbours(this, currentNode, mask, ref dirs);
                Console.WriteLine(currentNode.Location + ":" + currentNode.Dir + ":" + dirs);
                for (byte dir = 0; dir < _instance.DirectionCount(); dir++)
                    if (((1 << dir) & dirs) > 0)
                    {
                        var nc = NextNode(currentNode, dir);
                        if (_close.Contains(nc.Location))
                        {
                            _nodePool.Enqueue(nc);
                            continue;
                        }

                        var index = Jump(currentNode, nc, goal, currentNode.Dir, mask);
                        if (index == null) continue;

                        Console.WriteLine("index:" + index.Location + ",dir:" + index.Dir);
                        Debug.LogError("index:" + index.Location + ",dir:" + index.Dir);
                        if (index.Location == goal) return Trace(index, ref result);

                        if (!_openList.ContainsKey(index.Location))
                        {
                            _open.Enqueue(index, index.Cost);
                            _openList.Add(index.Location, index);
                        }
                        else if (_openList[index.Location].Cost > index.Cost)
                        {
                            _openList[index.Location].Update(index);
                            _open.UpdatePriority(_openList[index.Location], index.Cost);
                        }
                    }
            }

            return false;
        }

        private JPSNode Jump(JPSNode node, JPSNode nc, Vector2Int goal, byte dir, int mask)
        {
            var extendDir = _instance.ExtendDir(dir, node.Dir);

            JPSNode newP = null;
            byte dirs = 0;
            while (true)
            {
                Console.WriteLine(node.Location + ":" + nc.Location + "," + dir);
                if (!_grid._N(nc.Location.x, nc.Location.y, mask))
                {
                    _nodePool.Enqueue(nc);
                    return null;
                }

                dirs = 0;
                if (_instance.ForcedNeighbours(this, nc, mask, ref dirs) || nc.Location == goal) return nc;

                for (var i = 0; i < extendDir.Length; i++)
                    if (_grid.GetJumpPoint(this, _instance, nc, goal, extendDir[i], mask, out newP))
                    {
                        _nodePool.Enqueue(newP);
                        return nc;
                    }

                // Console.WriteLine(nc.Location +","+dir+":"+_instance.GetDirection(nc.Location.x, nc.Location.y, dir));
                Debug.LogError(nc.Location +","+nc.Dir);
                nc.Offset(_instance.GetDirection(nc.Location, nc.Dir));
            }
        }

        /// <summary>
        ///     回溯生成路径
        /// </summary>
        /// <param name="node"></param>
        /// <returns></returns>
        private bool Trace(JPSNode node, ref List<Vector2Int> result)
        {
            result.Clear();
            result.Add(node.Location);
            while (node.Parent != null)
            {
                Debug.LogError("parent:"+node.Location);
                var tmp = node.Parent;
                result.Add(node.Location);
                for (var i = node.Source.Count-1; i >= 0; i--)
                {
                    result.Add(node.Source[i]);
                }
                node = tmp;
            }

            result.Reverse();
            for (var i = 0; i < result.Count; i++)
            {
                // Debug.LogError("node"+result[i]);
            }
            return true;
        }
        
    }

    public class JPSGrid
    {
        protected readonly JPSNode[,] _grid;

        // 实际内容
        protected readonly int[,] _navigable;

        // 边界设定
        public readonly int BoundsMaxX;
        public readonly int BoundsMaxY;
        public readonly int BoundsMinX;
        public readonly int BoundsMinY;

        public JPSGrid(int[,] navigable)
        {
            BoundsMinX = 0;
            BoundsMaxX = navigable.GetUpperBound(0);
            BoundsMinY = 0;
            BoundsMaxY = navigable.GetUpperBound(1);
            _navigable = navigable;

            _grid = new JPSNode[BoundsMaxX + 1, BoundsMaxY + 1];
            for (var x = BoundsMinX; x <= BoundsMaxX; x++)
            for (var y = BoundsMinY; y <= BoundsMaxY; y++)
                _grid[x, y] = new JPSNode(x, y);
        }

        internal JPSNode this[int x, int y] => _grid[x, y];
        internal JPSNode this[Vector2Int location] => _grid[location.x, location.y];

        public void Update(Vector2Int point, int status)
        {
            _navigable[point.x, point.y] = status;
        }

        public void Clear()
        {
            if (_grid != null)
                foreach (var g in _grid)
                    g.Revert();
        }

        // 判断是否可以访问
        public bool _N(int x, int y, int mask)
        {
            return InBounds(x, y) && _navigable[x, y] == mask;
        }

        public int _S(int x, int y)
        {
            // Debug.LogError($"S : {x} {y}");
            return _navigable[x, y];
        }

        public bool InBounds(int x, int y)
        {
            return x >= BoundsMinX && x <= BoundsMaxX &&
                   y >= BoundsMinY && y <= BoundsMaxY;
        }

        public bool GetJumpPoint(JPS jps, JPSInterface instance, JPSNode node, Vector2Int goal, byte dir, int mask,
            out JPSNode r)
        {
            r = jps.NewNextNode(node.Location, dir);
            byte dirs = 0;
            while (true)
            {
                // Console.WriteLine(node.Location+"::"+r.Location+","+dir);
                if (!_N(r.Location.x, r.Location.y, mask)) return false;

                dirs = 0;
                if (instance.ForcedNeighbours(jps, r, mask, ref dirs) || r.Location == goal) return true;
                r.Offset(instance.GetDirection(r.Location, dir));
            }
        }

        // internal IEnumerable<JPSNode> Neighbours(JPSNode node, Vector2Int[] Directions)
        // {
        //     if (node.Parent != null)
        //     {
        //         // 有父亲节点，需要剪枝
        //         Vector2Int n = node.Location;
        //         Vector2Int p = node.Parent.Location;
        //         Vector2Int d = n - p;
        //         d.x /= Mathf.Max(Mathf.Abs(d.x), 1);
        //         d.y /= Mathf.Max(Mathf.Abs(d.y), 1);
        //         // 对角线
        //         if (d.x != 0 && d.y != 0)
        //         {
        //             // 轴向分量
        //             if (_N(n.x, n.y + d.y))
        //                 yield return this[n.x, n.y + d.y];
        //             if (_N(n.x + d.x, n.y))
        //                 yield return this[n.x + d.x, n.y];
        //             // 往前看
        //             if (_N(n.x, n.y + d.y) && _N(n.x + d.x, n.y) && _N(n.x + d.x, n.y + d.y))
        //                 yield return this[n.x + d.x, n.y + d.y];
        //         }
        //         // 垂直轴向
        //         else if (d.x == 0)
        //         {
        //             // 向前看
        //             if (_N(n.x, n.y + d.y))
        //                 yield return this[n.x, n.y + d.y];
        //             // 向左看
        //             if (!_N(n.x - 1, n.y - d.y) && _N(n.x - 1, n.y))
        //             {
        //                 yield return this[n.x - 1, n.y];
        //                 if (_N(n.x - 1, n.y + d.y))
        //                     yield return this[n.x - 1, n.y + d.y];
        //             }
        //             // 向右看
        //             if (!_N(n.x + 1, n.y - d.y) && _N(n.x + 1, n.y))
        //             {
        //                 yield return this[n.x + 1, n.y];
        //                 if (_N(n.x + 1, n.y + d.y))
        //                     yield return this[n.x + 1, n.y + d.y];
        //             }
        //         }
        //         // 水平轴向
        //         else if (d.y == 0)
        //         {
        //             if (_N(n.x + d.x, n.y))
        //                 yield return this[n.x + d.x, n.y];
        //             // 向左看
        //             if (!_N(n.x - d.x, n.y + 1) && _N(n.x, n.y + 1))
        //             {
        //                 yield return this[n.x, n.y + 1];
        //                 if (_N(n.x + d.x, n.y + 1))
        //                     yield return this[n.x + d.x, n.y + 1];
        //             }
        //             // 向右看
        //             if (!_N(n.x - d.x, n.y - 1) && _N(n.x, n.y - 1))
        //             {
        //                 yield return this[n.x, n.y - 1];
        //                 if (_N(n.x + d.x, n.y - 1))
        //                     yield return this[n.x + d.x, n.y - 1];
        //             }
        //         }
        //     }
        //     else
        //     {
        //         // 是起点
        //         for (var i = 0; i < 8; i++)
        //         {
        //             int propX = node.Location.x + Directions[i].x;
        //             int propY = node.Location.y + Directions[i].y;
        //
        //             if (_N(propX, propY))
        //             {
        //                 yield return this[propX, propY];
        //             }
        //         }
        //     }
        // }
    }

    public class GridNode : Node
    {
        public JPS _JPS;
        public Vector2Int _Point;

        public List<Edge> Connections()
        {
            var list = new List<Edge>();
            for (byte dir = 0; dir < _JPS._instance.DirectionCount(); dir++){
                var a = _JPS._instance.GetDirection(_Point, dir);
                var b = new Vector2Int(a.x + _Point.x, a.y + _Point.y);
                if (!_JPS._grid.InBounds(b.x, b.y))
                {
                    continue;
                }
                list.Add(new Edge
                {
                    ConnectedNode = new GridNode(){_JPS = _JPS, _Point = b},
                    Cost = 1
                });
            }
            return list;
        }

        public Vector2Int Point()
        {
            return _Point;
        }

        public int Status()
        {
            return _JPS._grid._S(_Point.x, _Point.y);
        }
        
        public double Distance(Node end)
        {
            return _JPS._instance.Distance(_Point, end.Point());
        }
    }
}